#ifndef RTDEPROTOCOL_H
#define RTDEPROTOCOL_H

#include <vector>
#include <memory>
#include <string>

namespace CURCONTROL {
static const int DASHBOARD_PORT = 29999;
static const int PRIMARY_PORT = 30001;
static const int SECONDARY_PORT = 30002;
static const int REALTIME_PORT = 30003;
static const int RTDE_PORT = 30004;
static const double UR_JOINT_VELOCITY_MAX = 3.14;// rad/s
static const double UR_JOINT_VELOCITY_MIN = 0.0; // rad/s
static const double UR_JOINT_ACCELERATION_MAX = 40.0;
static const double UR_JOINT_ACCELERATION_MIN = 0.0;
static const double UR_SERVO_LOOKAHEAD_TIME_MAX = 0.2;
static const double UR_SERVO_LOOKAHEAD_TIME_MIN = 0.03;
static const int UR_SERVO_GAIN_MAX = 2000;
static const int UR_SERVO_GAIN_MIN = 100;
static const std::string SCRIPT_PATH = "../rtde_control_borns.script";

/*!
* \brief robot status
*/
enum RobotStatus{
    ROBOT_STATUS_POWER_ON = 0,
    ROBOT_STATUS_PROGRAM_RUNNING = 1,
    ROBOT_STATUS_TEACH_BUTTON_PRESSED = 2,
    ROBOT_STATUS_POWER_BUTTON_PRESSED = 3
};

/*!
* \brief robot mode
*/
enum RobotMode
{
    ROBOT_MODE_NO_CONTROLLER = -1,
    ROBOT_MODE_DISCONNECTED = 0,
    ROBOT_MODE_CONFIRM_SAFETY,
    ROBOT_MODE_BOOTING,
    ROBOT_MODE_POWER_OFF,
    ROBOT_MODE_POWER_ON,
    ROBOT_MODE_IDLE,
    ROBOT_MODE_BACKDRIVE,
    ROBOT_MODE_RUNNING,
    ROBOT_MODE_UPDATING_FIRMWARE,
    ROBOT_MODE_UNKNOW
};
/*!
* \brief safety mode
*/
enum SafetyMode
{
    SAFETY_MODE_NORMAL = 1,
    SAFETY_MODE_REDUCED,
    SAFETY_MODE_PROTECTIVE_STOP,
    SAFETY_MODE_RECOVERY,
    SAFETY_MODE_SAFEGUARD_STOP,
    SAFETY_MODE_SYSTEM_EMERGENCY_STOP,
    SAFETY_MODE_ROBOT_EMERGENCY_STOP,
    SAFETY_MODE_VIOLATION,
    SAFETY_MODE_FAULT,
    SAFETY_MODE_VALIDATE_JOINT_ID,
    SAFETY_MODE_UNDEFINED_SAFETY_MODE = 11
};
/*!
* \brief UR的program state
*/
enum RuntimeState
{
    STOPPING = 0,
    STOPPED = 1,
    PLAYING = 2,
    PAUSING = 3,
    PAUSED = 4,
    RESUMING = 5
};
/*!
* \brief RTDE控制脚本命令执行状态
*/
enum ControlScriptState
{
    UR_CONTROLLER_UNKNOW = 0,
    UR_CONTROLLER_RDY_FOR_CMD = 1,
    UR_CONTROLLER_DONE_WITH_CMD = 2
};

/*!
* \brief RTDE输出配方变量类型
*/
enum OutVariableType{
    UINT32,
    UINT64,
    INT32,
    DOUBLE,
    VECTOR6D,
    VECTOR6INT32
};
/*!
* \brief RTDE所有的输出配方项结构体
* \brief name:输出配方项变量名
* \brief type_:输出配方项变量类型
* \brief variableSize:输出配方项变量的字节长度
*/
struct OutputType{
    std::string name_;
    OutVariableType type_;
    size_t variableSize_;
    OutputType(std::string name, OutVariableType type, size_t size){
        name_ = name;
        type_ = type;
        variableSize_ = size;
    }
};
/*!
* \brief RTDE所有的输出配方项数组，已注释的项表示目前RTDE协议未使用
* \brief 需要新增输出项的话，只需要将已注释的代码按格式还原即可
*/
static const std::vector<OutputType> OUTPUT_VARIABLES = {
    {"timestamp", OutVariableType::DOUBLE, sizeof (double) },
    //    { "target_q", typeid(std::vector<double>).hash_code() },
    //    { "target_qd", typeid(std::vector<double>).hash_code() },
    //    { "target_qdd", typeid(std::vector<double>).hash_code() },
    //    { "target_current", typeid(std::vector<double>).hash_code() },
    //    { "target_moment", typeid(std::vector<double>).hash_code() },
    { "actual_q", OutVariableType::VECTOR6D, sizeof(double) * 6 },
    //    { "actual_qd", typeid(std::vector<double>).hash_code() },
    //    { "actual_qdd", typeid(std::vector<double>).hash_code() },
    { "actual_current", OutVariableType::VECTOR6D, sizeof(double) * 6 },
    //    { "actual_moment", typeid(std::vector<double>).hash_code() },
    //    { "joint_control_output", typeid(std::vector<double>).hash_code() },
    { "actual_TCP_pose", OutVariableType::VECTOR6D, sizeof(double) * 6 },
    //    { "actual_TCP_speed", typeid(std::vector<double>).hash_code() },
    { "actual_TCP_force", OutVariableType::VECTOR6D, sizeof(double) * 6 },
    //    { "target_TCP_pose", typeid(std::vector<double>).hash_code() },
    //    { "target_TCP_speed", typeid(std::vector<double>).hash_code() },
    { "actual_digital_input_bits", OutVariableType::UINT64, sizeof(uint64_t) },
    { "joint_temperatures", OutVariableType::VECTOR6D, sizeof(double) * 6 },
    //    { "actual_execution_time", typeid(double).hash_code() },
    { "robot_mode", OutVariableType::INT32, sizeof(int32_t) },
    { "joint_mode", OutVariableType::VECTOR6INT32, sizeof(int32_t) * 6 },
    { "safety_mode", OutVariableType::INT32, sizeof(int32_t) },
    //    { "actual_tool_accelerometer", typeid(std::vector<double>).hash_code() },
    //    { "speed_scaling", typeid(double).hash_code() },
    //    { "target_speed_fraction", typeid(double).hash_code() },
    //    { "actual_momentum", typeid(double).hash_code() },
    //    { "actual_main_voltage", typeid(double).hash_code() },
    //    { "actual_robot_voltage", typeid(double).hash_code() },
    //    { "actual_robot_current", typeid(double).hash_code() },
    //    { "actual_joint_voltage", typeid(std::vector<double>).hash_code() },
    { "actual_digital_output_bits", OutVariableType::UINT64, sizeof(uint64_t) },
    { "runtime_state", OutVariableType::UINT32, sizeof(uint32_t) },
    { "robot_status_bits", OutVariableType::UINT32, sizeof(uint32_t) },
    { "safety_status_bits", OutVariableType::UINT32, sizeof(uint32_t) },
    //    { "standard_analog_input0", typeid(double).hash_code() },
    //    { "standard_analog_input1", typeid(double).hash_code() },
    //    { "standard_analog_output0", typeid(double).hash_code() },
    //    { "standard_analog_output1", typeid(double).hash_code() },
    //    { "ft_raw_wrench", typeid(std::vector<double>).hash_code() },
    //    { "payload", typeid(double).hash_code() },
    //    { "payload_cog", typeid(std::vector<double>).hash_code() },
    //    { "payload_inertia", typeid(std::vector<double>).hash_code() },
    //    { "output_bit_registers0_to_31", typeid(uint32_t).hash_code() },
    //    { "output_bit_registers32_to_63", typeid(uint32_t).hash_code() },
        { "output_int_register_0", OutVariableType::INT32, sizeof(int32_t) }
    //    { "output_int_register_1", typeid(int32_t).hash_code() },
    //    { "output_int_register_2", typeid(int32_t).hash_code() },
    //    { "output_int_register_3", typeid(int32_t).hash_code() },
    //    { "output_int_register_4", typeid(int32_t).hash_code() },
    //    { "output_int_register_5", typeid(int32_t).hash_code() },
    //    { "output_int_register_6", typeid(int32_t).hash_code() },
    //    { "output_int_register_7", typeid(int32_t).hash_code() },
    //    { "output_int_register_8", typeid(int32_t).hash_code() },
    //    { "output_int_register_9", typeid(int32_t).hash_code() },
    //    { "output_int_register_10", typeid(int32_t).hash_code() },
    //    { "output_int_register_11", typeid(int32_t).hash_code() },
    //    { "output_int_register_12", typeid(int32_t).hash_code() },
    //    { "output_int_register_13", typeid(int32_t).hash_code() },
    //    { "output_int_register_14", typeid(int32_t).hash_code() },
    //    { "output_int_register_15", typeid(int32_t).hash_code() },
    //    { "output_int_register_16", typeid(int32_t).hash_code() },
    //    { "output_int_register_17", typeid(int32_t).hash_code() },
    //    { "output_int_register_18", typeid(int32_t).hash_code() },
    //    { "output_int_register_19", typeid(int32_t).hash_code() },
    //    { "output_int_register_20", typeid(int32_t).hash_code() },
    //    { "output_int_register_21", typeid(int32_t).hash_code() },
    //    { "output_int_register_22", typeid(int32_t).hash_code() },
    //    { "output_int_register_23", typeid(int32_t).hash_code() },
    //    { "output_int_register_24", typeid(int32_t).hash_code() },
    //    { "output_int_register_25", typeid(int32_t).hash_code() },
    //    { "output_int_register_26", typeid(int32_t).hash_code() },
    //    { "output_int_register_27", typeid(int32_t).hash_code() },
    //    { "output_int_register_28", typeid(int32_t).hash_code() },
    //    { "output_int_register_29", typeid(int32_t).hash_code() },
    //    { "output_int_register_30", typeid(int32_t).hash_code() },
    //    { "output_int_register_31", typeid(int32_t).hash_code() },
    //    { "output_int_register_32", typeid(int32_t).hash_code() },
    //    { "output_int_register_33", typeid(int32_t).hash_code() },
    //    { "output_int_register_34", typeid(int32_t).hash_code() },
    //    { "output_int_register_35", typeid(int32_t).hash_code() },
    //    { "output_int_register_36", typeid(int32_t).hash_code() },
    //    { "output_int_register_37", typeid(int32_t).hash_code() },
    //    { "output_int_register_38", typeid(int32_t).hash_code() },
    //    { "output_int_register_39", typeid(int32_t).hash_code() },
    //    { "output_int_register_40", typeid(int32_t).hash_code() },
    //    { "output_int_register_41", typeid(int32_t).hash_code() },
    //    { "output_int_register_42", typeid(int32_t).hash_code() },
    //    { "output_int_register_43", typeid(int32_t).hash_code() },
    //    { "output_int_register_44", typeid(int32_t).hash_code() },
    //    { "output_int_register_45", typeid(int32_t).hash_code() },
    //    { "output_int_register_46", typeid(int32_t).hash_code() },
    //    { "output_int_register_47", typeid(int32_t).hash_code() },
    //    { "output_double_register_0", typeid(double).hash_code() },
    //    { "output_double_register_1", typeid(double).hash_code() },
    //    { "output_double_register_2", typeid(double).hash_code() },
    //    { "output_double_register_3", typeid(double).hash_code() },
    //    { "output_double_register_4", typeid(double).hash_code() },
    //    { "output_double_register_5", typeid(double).hash_code() },
    //    { "output_double_register_6", typeid(double).hash_code() },
    //    { "output_double_register_7", typeid(double).hash_code() },
    //    { "output_double_register_8", typeid(double).hash_code() },
    //    { "output_double_register_9", typeid(double).hash_code() },
    //    { "output_double_register_10", typeid(double).hash_code() },
    //    { "output_double_register_11", typeid(double).hash_code() },
    //    { "output_double_register_12", typeid(double).hash_code() },
    //    { "output_double_register_13", typeid(double).hash_code() },
    //    { "output_double_register_14", typeid(double).hash_code() },
    //    { "output_double_register_15", typeid(double).hash_code() },
    //    { "output_double_register_16", typeid(double).hash_code() },
    //    { "output_double_register_17", typeid(double).hash_code() },
    //    { "output_double_register_18", typeid(double).hash_code() },
    //    { "output_double_register_19", typeid(double).hash_code() },
    //    { "output_double_register_20", typeid(double).hash_code() },
    //    { "output_double_register_21", typeid(double).hash_code() },
    //    { "output_double_register_22", typeid(double).hash_code() },
    //    { "output_double_register_23", typeid(double).hash_code() },
    //    { "output_double_register_24", typeid(double).hash_code() },
    //    { "output_double_register_25", typeid(double).hash_code() },
    //    { "output_double_register_26", typeid(double).hash_code() },
    //    { "output_double_register_27", typeid(double).hash_code() },
    //    { "output_double_register_28", typeid(double).hash_code() },
    //    { "output_double_register_29", typeid(double).hash_code() },
    //    { "output_double_register_30", typeid(double).hash_code() },
    //    { "output_double_register_31", typeid(double).hash_code() },
    //    { "output_double_register_32", typeid(double).hash_code() },
    //    { "output_double_register_33", typeid(double).hash_code() },
    //    { "output_double_register_34", typeid(double).hash_code() },
    //    { "output_double_register_35", typeid(double).hash_code() },
    //    { "output_double_register_36", typeid(double).hash_code() },
    //    { "output_double_register_37", typeid(double).hash_code() },
    //    { "output_double_register_38", typeid(double).hash_code() },
    //    { "output_double_register_39", typeid(double).hash_code() },
    //    { "output_double_register_40", typeid(double).hash_code() },
    //    { "output_double_register_41", typeid(double).hash_code() },
    //    { "output_double_register_42", typeid(double).hash_code() },
    //    { "output_double_register_43", typeid(double).hash_code() },
    //    { "output_double_register_44", typeid(double).hash_code() },
    //    { "output_double_register_45", typeid(double).hash_code() },
    //    { "output_double_register_46", typeid(double).hash_code() },
    //    { "output_double_register_47", typeid(double).hash_code() }
};

/*!
* \brief rtde协议类
*/
class RTDEProtocol
{
public:
    // 报头大小
    static const unsigned HEADER_SIZE = 3;
    // 获取输出配方项的名称集合
    static std::vector<std::string> GetOutputNames(){
        std::vector<std::string> rtn;
        for(const auto &ele:OUTPUT_VARIABLES){
            rtn.emplace_back(ele.name_);
        }
        return rtn;
    }
    RTDEProtocol(){

    }
    /*!
   * \brief 控制命令类型
   */
    enum Type
    {
        NO_CMD = 0,
        MOVEJ = 1,
        MOVEJ_IK = 2,
        MOVEL = 3,
        MOVEL_FK = 4,
        FORCE_MODE = 6,
        FORCE_MODE_STOP = 7,
        ZERO_FT_SENSOR = 8,
        SPEEDJ = 9,
        SPEEDL = 10,
        SERVOJ = 11,
        SERVOC = 12,
        SET_STD_DIGITAL_OUT = 13,
        SET_TOOL_DIGITAL_OUT = 14,
        SPEED_STOP = 15,
        SERVO_STOP = 16,
        SET_PAYLOAD = 17,
        TEACH_MODE = 18,
        END_TEACH_MODE = 19,
        FORCE_MODE_SET_DAMPING = 20,
        FORCE_MODE_SET_GAIN_SCALING = 21,
        SET_SPEED_SLIDER = 22,
        SET_STD_ANALOG_OUT = 23,
        SERVOJ_POS = 24,
        TOOL_CONTACT = 25,
        GET_STEPTIME = 26,
        GET_ACTUAL_JOINT_POSITIONS_HISTORY = 27,
        GET_TARGET_WAYPOINT = 28,
        SET_TCP = 29,
        GET_INVERSE_KINEMATICS_ARGS = 30,
        PROTECTIVE_STOP = 31,
        STOPL = 33,
        STOPJ = 34,
        SET_WATCHDOG = 35,
        IS_POSE_WITHIN_SAFETY_LIMITS = 36,
        IS_JOINTS_WITHIN_SAFETY_LIMITS = 37,
        GET_JOINT_TORQUES = 38,
        POSE_TRANS = 39,
        GET_TCP_OFFSET = 40,
        JOG_START = 41,
        JOG_STOP = 42,
        GET_FORWARD_KINEMATICS_DEFAULT = 43,
        GET_FORWARD_KINEMATICS_ARGS = 44,
        MOVE_PATH = 45,
        GET_INVERSE_KINEMATICS_DEFAULT = 46,
        IS_STEADY = 47,
        SET_CONF_DIGITAL_OUT = 48,
        SET_INPUT_INT_REGISTER = 49,
        SET_INPUT_DOUBLE_REGISTER = 50,
        MOVE_UNTIL_CONTACT = 51,
        FREEDRIVE_MODE = 52,
        END_FREEDRIVE_MODE = 53,
        GET_FREEDRIVE_STATUS = 54,
        SET_EXTERNAL_FORCE_TORQUE = 55,
        FT_RTDE_INPUT_ENABLE = 56,
        ENABLE_EXTERNAL_FT_SENSOR = 57,
        GET_ACTUAL_TOOL_FLANGE_POSE = 58,
        SET_GRAVITY = 59,
        GET_INVERSE_KINEMATICS_HAS_SOLUTION_DEFAULT = 60,
        GET_INVERSE_KINEMATICS_HAS_SOLUTION_ARGS = 61,
        START_CONTACT_DETECTION = 62,
        STOP_CONTACT_DETECTION = 63,
        READ_CONTACT_DETECTION = 64,
        SET_TOOL_VOLTAGE = 99,
        STOP_SCRIPT = 255
    };

    /*!
   * \brief rtde输入配方id
   */
    enum Recipe
    {
        RECIPE_1 = 1,
        RECIPE_2 = 2,
        RECIPE_3 = 3,
        RECIPE_4 = 4,
        RECIPE_5 = 5,
        RECIPE_6 = 6,
        RECIPE_7 = 7,
        RECIPE_8 = 8,
        RECIPE_9 = 9,
        RECIPE_10 = 10,
        RECIPE_11 = 11,
        RECIPE_12 = 12,
        RECIPE_13 = 13,
        RECIPE_14 = 14,
        RECIPE_15 = 15,
        RECIPE_16 = 16,
        RECIPE_17 = 17,
        RECIPE_18 = 18,
        RECIPE_19 = 19,
        RECIPE_20 = 20
    };

    Type type_ = NO_CMD;
    std::uint8_t recipe_id_;
    std::int32_t async_;
    std::int32_t ft_rtde_input_enable_;
    std::int32_t reg_int_val_;
    double reg_double_val_;
    std::vector<double> val_;
    std::vector<int> selection_vector_;
    std::vector<int> free_axes_;
    std::int32_t force_mode_type_;
    std::uint8_t std_digital_out_;
    std::uint8_t std_digital_out_mask_;
    std::uint8_t configurable_digital_out_;
    std::uint8_t configurable_digital_out_mask_;
    std::uint8_t std_tool_out_;
    std::uint8_t std_tool_out_mask_;
    std::uint8_t std_analog_output_mask_;
    std::uint8_t std_analog_output_type_;
    double std_analog_output_0_;
    double std_analog_output_1_;
    std::int32_t speed_slider_mask_;
    double speed_slider_fraction_;
    std::uint32_t steps_;
};


/*!
* \brief rtde协议命令类型
*/
enum RTDECommand
{
    RTDE_REQUEST_PROTOCOL_VERSION = 86,       // ascii V
    RTDE_GET_URCONTROL_VERSION = 118,         // ascii v
    RTDE_TEXT_MESSAGE = 77,                   // ascii M
    RTDE_DATA_PACKAGE = 85,                   // ascii U
    RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79,  // ascii O
    RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 73,   // ascii I
    RTDE_CONTROL_PACKAGE_START = 83,          // ascii S
    RTDE_CONTROL_PACKAGE_PAUSE = 80           // ascii P
};

}// namespace CURCONTROL

#endif // RTDEPROTOCOL_H
